#include "motor_control_utils.h"

#include <rclcpp/rclcpp.hpp>
#include <thread>
#include <vector>

using namespace std;

constexpr char CAN0_INTERFACE[] = "can0";
constexpr char CAN1_INTERFACE[] = "can1";
constexpr int CAN_BITRATE = 250000;
map<MotorType, unique_ptr<MotorController>> MotorControlUtils::m_motors;

void MotorControlUtils::motorControllerInit() {
  auto can0 = make_shared<CANopenDriver>(CAN0_INTERFACE, CAN_BITRATE);
  auto can1 = make_shared<CANopenDriver>(CAN1_INTERFACE, CAN_BITRATE);

  m_motors[MotorType::FRONT_LEFT_POWER_MOTOR] =
      make_unique<PowerMotorController>(1, can0);
  m_motors[MotorType::FRONT_LEFT_STEERING_MOTOR] =
      make_unique<SteeringMotorController>(2, can0);
  m_motors[MotorType::FRONT_RIGHT_POWER_MOTOR] =
      make_unique<PowerMotorController>(3, can0);
  m_motors[MotorType::FRONT_RIGHT_STEERING_MOTOR] =
      make_unique<SteeringMotorController>(4, can0);
  m_motors[MotorType::REAR_LEFT_POWER_MOTOR] =
      make_unique<PowerMotorController>(5, can1);
  m_motors[MotorType::REAR_LEFT_STEERING_MOTOR] =
      make_unique<SteeringMotorController>(6, can1);
  m_motors[MotorType::REAR_RIGHT_POWER_MOTOR] =
      make_unique<PowerMotorController>(7, can1);
  m_motors[MotorType::REAR_RIGHT_STEERING_MOTOR] =
      make_unique<SteeringMotorController>(8, can1);

  vector<thread> threads;
  for (const auto& iter : m_motors) {
    threads.emplace_back([&iter]() { iter.second->powerOnSelfCheck(); });
  }
  for (auto& t : threads) {
    t.join();
  }
  for (const auto& iter : m_motors) {
    iter.second->init();
  }
}

map<MotorType, unique_ptr<MotorController>>&
MotorControlUtils::getController() {
  if (m_motors.empty()) {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "get motor controller empty");
    motorControllerInit();
  }
  return m_motors;
}